//#include "debug.h"
//#include "QWF_Ctrl.h"
//#include "usart.h"
//#include "QWF_Ctrl.h"
////放在定时1ms的freertos/tim里
//#if debug_type==Chassis_Power_Debug//底盘功率
//#include "motor.h"
//#include "pid.h"
//#include "arm_math.h"
////#include "chassis.h"
////输出常见情况的电机输出情况各2s
//void motor_debug(Send_Data_To_Motor send_func,PID* pid,float meas){//速度环控制
//	//固定转速，-200rad/s,-100rad/s,-50rad/s,-20rad/s,-10rad/s,\
//				0rad/s,10,20,50,100,200各2s
//	static uint32_t tick=0;
//	tick++;
//	if(tick<2000)
//		send_func(PID_Calculate(pid,-200*60/(2*PI),meas));
//	else if(tick<4000)
//		send_func(PID_Calculate(pid,-100*60/(2*PI),meas));
//	else if(tick<6000)
//		send_func(PID_Calculate(pid,-50*60/(2*PI),meas));
//	else if(tick<8000)
//		send_func(PID_Calculate(pid,-20*60/(2*PI),meas));
//	else if(tick<10000)
//		send_func(PID_Calculate(pid,-10*60/(2*PI),meas));
//	else if(tick<12000)
//		send_func(PID_Calculate(pid,-0*60/(2*PI),meas));
//	else if(tick<14000)
//		send_func(PID_Calculate(pid,10*60/(2*PI),meas));
//	else if(tick<16000)
//		send_func(PID_Calculate(pid,20*60/(2*PI),meas));
//	else if(tick<18000)
//		send_func(PID_Calculate(pid,50*60/(2*PI),meas));
//	else if(tick<20000)
//		send_func(PID_Calculate(pid,100*60/(2*PI),meas));
//	else if(tick<22000)
//		send_func(PID_Calculate(pid,200*60/(2*PI),meas));
//	//频率5Hz,10Hz,20Hz,幅度1A,2A,5A正弦波运动（各两个周期）
//	else if(tick<26000)
//		send_func(1.0f*arm_sin_f32(10.0f*PI*(tick-22000)/1000.0f));
//	else if(tick<30000)
//		send_func(2.0f*arm_sin_f32(20.0f*PI*(tick-22000)/1000.0f));		
//	else if(tick<34000)
//		send_func(5.0f*arm_sin_f32(30.0f*PI*(tick-22000)/1000.0f));		
//	else 
//		send_func(0);		
//	return;
//}
//void angle_motor_debug(Send_Data_To_Motor send_func,PID* pid_a,PID* pid_w,float meas_a,float meas_w){//角度换控制
//	//跃迁90，80，70，60，50，40，30，20，10，0，-10，-20。。。。。-90，每次停留到达后停留2s
//	float tar_a=0;
//		static uint32_t tick=0;
//	tick++;
//	if(tick<1000*3)
//		tar_a=10*8192/360.0f;
//	else if(tick<2000*3)
//		tar_a=0*8192/360.0f;
//	else if(tick<3000*3)
//		tar_a=-20*8192/360.0f;
//	else if(tick<4000*3)
//		tar_a=10*8192/360.0f;
//	else if(tick<5000*3)
//		tar_a=-30*8192/360.0f;
//	else if(tick<6000*3)
//		tar_a=20*8192/360.0f;
//	else if(tick<7000*3)
//		tar_a=-40*8192/360.0f;
//	else if(tick<8000*3)
//		tar_a=30*8192/360.0f;
//	else if(tick<9000*3)
//		tar_a=-50*8192/360.0f;
//	else if(tick<10000*3)
//		tar_a=40*8192/360.0f;
//	else if(tick<11000*3)
//		tar_a=-60*8192/360.0f;
//	else if(tick<12000*3)
//		tar_a=50*8192/360.0f;
//	else if(tick<13000*3)
//		tar_a=-70*8192/360.0f;
//	else if(tick<14000*3)
//		tar_a=80*8192/360.0f;
//	else if(tick<15000*3)
//		tar_a=-90*8192/360.0f;
//	else if(tick<16000*3)
//		tar_a=90*8192/360.0f;
//	//频率5Hz,10Hz,20Hz,幅度1A,2A,5A正弦波运动（各两个周期）
//	else if(tick<36000*3)
//		tar_a=(10.0f*arm_sin_f32(10.0f*PI*(tick-36000)/1000.0f))*8192/360.0f;
//	else if(tick<56000*3)
//		tar_a=(20.0f*arm_sin_f32(20.0f*PI*(tick-36000)/1000.0f))*8192/360.0f;		
//	else if(tick<76000*3)
//		tar_a=(50.0f*arm_sin_f32(30.0f*PI*(tick-36000)/1000.0f))*8192/360.0f;		
//	else 
//		tar_a=(0)*8192/360.0f;	
//	float w=PID_Calculate(pid_a,meas_a,find_min_angle(tar_a,meas_a,8192));	
//	send_func(PID_Calculate(pid_w,meas_w,w));
//	
//}
//typedef struct{
//uint8_t head;
//float p;
//float rpm;
//float i;	
//}__attribute__((packed)) data_tx_power;
//data_tx_power QWF_data_Tx_Power={.head=0xAB};
//void debug_task(void const * argument){//can统一发送任务
//	static TickType_t xLastWakeTime;
//    const TickType_t xPeriod = pdMS_TO_TICKS(1);// 1ms周期
//    // 初始化唤醒时间
//    xLastWakeTime = xTaskGetTickCount();
//	while(1)
//	{	/********************************CAN1*************************/
////		motor_debug(send_data_to_chassis_motor->b_wheel,chassis_pid->b.v,Chassis_B_c620.rpm);
//		angle_motor_debug(send_data_to_chassis_motor->f_helm,chassis_pid->f.angle,chassis_pid->f.w,chassis->part.f.helm.motor.rx_meas.angle[2],chassis->part.f.helm.motor.rx_meas.rpm);
//		Send_float_To_USART(&huart6,power_get->p);
//		Send_float_To_USART(&huart6,Chassis_F_gm6020.rx_meas.rpm);
//		Send_float_To_USART(&huart6,chassis_pid->f.w.output);
//		Send_float_To_USART(&huart6,Chassis_F_gm6020.rx_meas.torque);
//		
////		sprintf((char*)data,"%f%f\t",1.2,3.5);//(float)Chassis_B_c620.rpm,(float)Chassis_B_c620.torque);
////		((float*)(data_TX+1))[1]=1;
////		((float*)(data_TX+1))[2]=2;		
//		Send_Data_To_USART(&huart6,tail,4);
//	
//		vTaskDelayUntil(&xLastWakeTime, xPeriod);	
//	}
//}
//#elif debug_type==Chassis_Load_Debug
////#include "chassis.h"
//typedef struct{
//uint8_t head;
//float i_load;
//float w;
//}__attribute__((packed)) data_tx_yaw_load;
//data_tx_yaw_load Data_Tx_Taw_Load={.head=0xAB};
//void debug_task(void const * argument){//can统一发送任务
//	static TickType_t xLastWakeTime;
//    const TickType_t xPeriod = pdMS_TO_TICKS(1);// 1ms周期
//    // 初始化唤醒时间
//    xLastWakeTime = xTaskGetTickCount();
//	while(1)
//	{	/********************************CAN1*************************/
//			pitch_w_target=PID_Calculate(head_pid->gimbal.pitch.angle,pitch_angle_meas_gyro,0);
////			send_data_to_gimbal_motor->Pitch(PID_Calculate(head_pid->gimbal.pitch.w,pitch_w_meas_gyro,pitch_w_target));

//			send_data_to_chassis_motor->b_wheel(PID_Calculate(&chassis_pid->b.v,Chassis_B_c620.rpm,1*60/(2*PI)*8192/360.0f));
////		send_data_to_gimbal_motor->Yaw(a_debug);

//		Data_Tx_Taw_Load.i_load=Chassis_B_c620.torque;
//			Data_Tx_Taw_Load.w=Chassis_B_c620.rpm;
////		sprintf((char*)data,"%f%f\t",1.2,3.5);//(float)Chassis_B_c620.rpm,(float)Chassis_B_c620.torque);
////		((float*)(data_TX+1))[1]=1;
////		((float*)(data_TX+1))[2]=2;		
//		HAL_UART_Transmit(&huart6,(uint8_t*)&Data_Tx_Taw_Load,sizeof(Data_Tx_Taw_Load),0xffff);//缺点：慢
//	
//		vTaskDelayUntil(&xLastWakeTime, xPeriod);	
//	}
//}


//#elif debug_type==Yaw_Load_Debug
//#include "head.h"
// #define yaw_k_w	 6.62746173948446
// #define yaw_k_w2	 -0.01089959945646421
// #define yaw_k_0	2459.3572408859327
// #define w_yaw_esp	QWF_Esp(head->part.gimbal.yaw.motor.rpm,0)
// #define yaw_load	QWF_Signed(head->part.gimbal.yaw.motor.rpm,0)*(yaw_k_w*w_yaw_esp+ yaw_k_w2*w_yaw_esp*w_yaw_esp+yaw_k_0)
//typedef struct{
//uint8_t head;
//float i_load;
//float w;
//float angle;
//float my_load;
//}__attribute__((packed)) data_tx_yaw_load;
//data_tx_yaw_load Data_Tx_Taw_Load={.head=0xAB};
//int my_yaw_v=0;
//void debug_task(void const * argument){//can统一发送任务
//	static TickType_t xLastWakeTime;
//    const TickType_t xPeriod = pdMS_TO_TICKS(1);// 1ms周期
//    // 初始化唤醒时间
//    xLastWakeTime = xTaskGetTickCount();
//	while(1)
//	{	/********************************CAN1*************************/
//		Gimbal_State_Updata();
//	static uint32_t tick=0;
//	tick++;		
//			pitch_w_target=PID_Calculate(head_pid->gimbal.pitch.angle,head->my->angle_pitch,20.0f*arm_sin_f32(10.0f*PI*(tick-220000)/10000.0f));
////			pitch_w_target=PID_Calculate(head_pid->gimbal.pitch.angle,head->my->angle_pitch,0);

//		send_data_to_gimbal_motor->Pitch(PID_Calculate(head_pid->gimbal.pitch.w,head->my->w_pitch,pitch_w_target));

//			send_data_to_gimbal_motor->Yaw(PID_Calculate(head_pid->gimbal.yaw.w,yaw_w_meas_motor*360/8192.0f,my_yaw_v*60/(2*PI)));
////		send_data_to_gimbal_motor->Yaw(a_debug);
//		Send_float_To_USART(&huart6,head->part.gimbal.yaw.motor.torque);
//		Send_float_To_USART(&huart6,head->part.gimbal.yaw.motor.rpm);
//		Send_float_To_USART(&huart6,head->part.gimbal.yaw.motor.angle[0]);
//		Send_float_To_USART(&huart6,head_pid->gimbal.yaw.w->output);
//		
//		Send_float_To_USART(&huart6,yaw_load);
//		
////		Data_Tx_Taw_Load.i_load=head->part.gimbal.yaw.motor.torque;
////			Data_Tx_Taw_Load.w=head->part.gimbal.yaw.motor.rpm;
////			Data_Tx_Taw_Load.angle=head->part.gimbal.yaw.motor.angle[0];
////			Data_Tx_Taw_Load.my_load=yaw_load;
////		sprintf((char*)data,"%f%f\t",1.2,3.5);//(float)Chassis_B_c620.rpm,(float)Chassis_B_c620.torque);
////		((float*)(data_TX+1))[1]=1;
////		((float*)(data_TX+1))[2]=2;		
//		Send_Data_To_USART(&huart6,tail,4);

////		HAL_UART_Transmit(&huart6,(uint8_t*)&Data_Tx_Taw_Load,sizeof(Data_Tx_Taw_Load),0xffff);//缺点：慢
//		
//		vTaskDelayUntil(&xLastWakeTime, xPeriod);	
//	}
//}
//#elif debug_type==Pitch_G_Debug
//typedef struct{
//uint8_t head;
//float out;
//float angle;
//}__attribute__((packed)) data_tx_yaw_load;
//data_tx_yaw_load Data_Tx_Taw_Load={.head=0xAB};
//int16_t pitch_g_out=3000;
//float my_pitch_angle_test=0;
//void debug_task(void const * argument){//can统一发送任务
//	static TickType_t xLastWakeTime;
//    const TickType_t xPeriod = pdMS_TO_TICKS(1);// 1ms周期
//    // 初始化唤醒时间
//    xLastWakeTime = xTaskGetTickCount();
//	while(1)
//	{	/********************************CAN1*************************/
//		Gimbal_State_Updata();	
//		pitch_w_target=PID_Calculate(head_pid->gimbal.pitch.angle,head->my->angle_pitch,my_pitch_angle_test);
//		send_data_to_gimbal_motor->Pitch(PID_Calculate(head_pid->gimbal.pitch.w,head->my->w_pitch,pitch_w_target));

////			send_data_to_gimbal_motor->Yaw(PID_Calculate(head_pid->gimbal.yaw.w,yaw_w_meas_motor,1*60/(2*PI)*8192/360.0f));
////		send_data_to_gimbal_motor->Yaw(a_debug);
//		Send_float_To_USART(&huart6,head->part.gimbal.pitch.motor.torque);
//		Send_float_To_USART(&huart6,pitch_angle_meas_motor);

////		sprintf((char*)data,"%f%f\t",1.2,3.5);//(float)Chassis_B_c620.rpm,(float)Chassis_B_c620.torque);
////		((float*)(data_TX+1))[1]=1;
////		((float*)(data_TX+1))[2]=2;		
//		Send_Data_To_USART(&huart6,tail,4);
//	
//		vTaskDelayUntil(&xLastWakeTime, xPeriod);	
//	}
//}

//#endif
